#include <Servo.h>

/* Analog output pin */
int pin10 = 10; // movement x
int pin11 = 11; // movement y

//0 = none, 1 = robot_x, 2 = robot_y, 3 = camera_x 4 = camera_y
byte active_header;

byte prev_robot_x;
byte prev_robot_y;

void setup(){

	//opens serial port, sets data rate to 9600 bps
	Serial.begin(9600);

	//make the pins outputs
	pinMode(pin10, OUTPUT);
	pinMode(pin11, OUTPUT);

	prev_robot_x = 127;
	prev_robot_y = 127;

	active_header = 0;

}

void loop(){

	byte in_value;
	byte write_value_x;
	byte write_value_y;

	if (Serial.available() > 0) {
		//Serial.available() returns the number of bytes (characters) available for reading from the serial port. This is data that's already arrived and stored in the serial receive buffer (which holds 128 bytes). 

		//read a byte
		in_value = Serial.read();

		if(active_header == 0){

			//this is a header
			active_header = in_value;

		}else{

			//this is a value corresponding to the active header
			switch(active_header){
				case 1:
					//robot_x
					prev_robot_x = in_value;
					analogWrite(pin10, prev_robot_x);
					break;
				case 2:
					//robot_y
					prev_robot_y = in_value;
					analogWrite(pin11, prev_robot_y);
					break;
				case 3:
					//camera_x
					break;
				case 4:
					//camera_y
					break;
				default:
					break;
			}

			//next time we get a new header
			active_header = 0;
		}
	}else{
		//no new info
		//if the last command was not stationary, keep doing it
		//if(prev_robot_x != 127){
			analogWrite(pin10, prev_robot_x);
		//}else if(prev_robot_y != 127){
			analogWrite(pin11, prev_robot_y);
		//}
	}
}
